#ifndef MATRIX_H
#define MATRIX_H

#include "Vector.h"

struct  Matrix3
{
	Real elts_[3][3];

	void Fill(Real);
	void Diag(Real);

	// Make the matrix [VA|VB|VC]
	void SetVectors(const Vector3& va, const Vector3& vb, const Vector3& vc);
	Real operator ()(int i, int j)const { return elts_[i][j]; }
	Real &operator ()(int i, int j){ return elts_[i][j]; }
	Real Trace()const;
	Real Determinant()const;
	Real SubDeterminant(int, int)const;
	void Print()const;
	Real MaxNorm()const;
	Real HilbertSchmidtNorm()const;
	Real MaxRowSum()const;
	Real MaxColSum()const;

	void AddDiag(Real);
	static Real InnerProduct(const Matrix3&, const Matrix3&);
	static void Negate(Matrix3& dest, const Matrix3&);
	static void Square(Matrix3& dest, const Matrix3&);
	static void MultiplyMTM(Matrix3& dest, const Matrix3& m1, const Matrix3& m2);
	static void MultiplyMM(Matrix3& dest, const Matrix3& m1, const Matrix3& m2);
	static void Scale(Matrix3& dest, Real r, const Matrix3& m2);
	static void Subtract(Matrix3& dest, const Matrix3&, const Matrix3&);
	static void Add(Matrix3& dest, const Matrix3&, const Matrix3&);
	static void TransposeTo(Matrix3& dest, const Matrix3&);
	static void InvertTo(Matrix3& dest, const Matrix3&);
	static void InvertToWithDet(Matrix3& dest, const Matrix3&, Real);
	static void CofactorMatrix(Matrix3& dest, const Matrix3&);
	static void MultiplyMV(Vector3& dest, const Matrix3&m, const Vector3& rhs);
	static void Saxpy(Matrix3& dest, Real a, const Matrix3&, const Matrix3&);

	// Set rotation matrices angle is in radians
	static void Symmetrify(Matrix3& dest, const Matrix3& src);
	static void DoubleSymmetrify(Matrix3& dest, const Matrix3 &src);

	void SetRotate(int axis, Real angle);
	static void ToOpenglMatrix(Real *rp, const Matrix3& m);
	static void FromOpenglMatrix(Matrix3 *m, const Real *rp);
	static void FromOpenglMatrixf(Matrix3 *m, const float *rp);
	void operator *= (Real r);
	static Matrix3 RotateAbout(Vector3, Real);
};

inline Matrix3 operator * (const Matrix3& m1, const Matrix3& m2)
{
	Matrix3 ret;
	Matrix3::MultiplyMM(ret, m1, m2);
	return ret;
}

inline Vector3 operator * (const Matrix3&m1, const Vector3& v)
{
	Vector3 w;
	Matrix3::MultiplyMV(w, m1, v);
	return w;
}

inline Matrix3 operator + (const Matrix3 &m1, const Matrix3 &m2)
{
	Matrix3 r;
	Matrix3::Add(r, m1, m2);
	return r;
}

inline Matrix3 operator - (const Matrix3 &m1, const Matrix3 &m2)
{
	Matrix3 r;
	Matrix3::Subtract(r, m1, m2);
	return r;
}

inline Matrix3 operator * (Real r, const Matrix3 &m)
{
	Matrix3 ret;
	Matrix3::Scale(ret, r, m);
	return ret;
}

#endif
